//
// Created by JYL on 2024-06-17.
//
#include "pid.h"
#include "balance_task.h"
#include "filter_task.h"
#include "voltage_task.h"
#include "uart_task.h"

//当两个任务的执行频率为5ms时
float Move_X,Move_Z;                          //控制小车避障、跟随时前进的变量，转弯的变量
float Balance_Kp=63131,Balance_Kd=160,Velocity_Kp=438,Velocity_Ki=2,Turn_Kp=4200,Turn_Kd=100;//PID参数（放大100倍）
//使执行频率为1ms的话，很多低优先级任务直接就饿死了

/**************************************************************************
函数功能：直立PD控制
入口参数：Angle:角度；Gyro：角速度
返回  值：balance：直立控制PWM
**************************************************************************/
int Balance(float Angle,float Gyro)
{
    float Angle_bias,Gyro_bias;
    int balance_pwm;
    Angle_bias=balance.Middle_angle-Angle;                       				//求出平衡的角度中值 和机械相关
    Gyro_bias=0-Gyro;
    balance_pwm=-Balance_Kp/100*Angle_bias-Gyro_bias*Balance_Kd/100; //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
    balance.Middle_angle -= Angle_bias*0.001;    //自适应重心
    return balance_pwm;
}


/**************************************************************************
函数功能：速度控制PWM
入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数
返回  值：速度控制PWM
**************************************************************************/
//修改前进后退速度，请修改Target_Velocity，比如，改成60就比较慢了
int Velocity(int encoder_left,int encoder_right)
{
    static float velocity,Encoder_Least,Encoder_bias,Movement;
    static float Encoder_Integral;
    //================遥控前进后退部分====================//
    if(Flag_front==1)    	Movement=Target_Velocity/Perimeter/Control_Frequency*EncoderMultiples*Reduction_Ratio*Encoder_precision*2;	  //收到前进信号
    else if(Flag_back==1)	Movement=-Target_Velocity/Perimeter/Control_Frequency*EncoderMultiples*Reduction_Ratio*Encoder_precision*2;  //收到后退信号
    else  Movement=Move_X/Perimeter/Control_Frequency*EncoderMultiples*Reduction_Ratio*Encoder_precision*2;//将给定速度转化为电机的编码器读数单位;
    /*Movement=Move_X/周长/编码器读取频率*频数*减速比*精度*2;*/
    if(Movement>2400)  Movement=2400;                     //蓝牙遥控速度的限制，避免破坏小车的平衡

    //================速度PI控制器=====================//
    Encoder_Least =0-(encoder_left+encoder_right);                    //获取最新速度偏差=目标速度（此处为零）-测量速度（左右编码器之和）
    Encoder_bias *= 0.84;		                                          //一阶低通滤波器
    Encoder_bias += Encoder_Least*0.16;	                              //一阶低通滤波器，减缓速度变化
    Encoder_Integral +=Encoder_bias;                                  //积分出位移 积分时间：10ms
    Encoder_Integral=Encoder_Integral+Movement;                       //接收遥控器数据，控制前进后退
    if(Encoder_Integral>200000)  	Encoder_Integral=200000;            //积分限幅
    if(Encoder_Integral<-200000)	  Encoder_Integral=-200000;         //积分限幅
    velocity=-Encoder_bias*Velocity_Kp/100-Encoder_Integral*Velocity_Ki/100;     //速度控制
    //Move_X	=0;
    if(Turn_Off(mpu6050.Angle_Balance,adc.battery_voltage)==1||balance.Flag_Stop==1) Encoder_Integral=0;//电机关闭后清除积分
    return velocity;
}
/**************************************************************************
函数功能：转向控制
入口参数：Z轴陀螺仪
返回  值：转向控制PWM
作    者：轮趣科技（东莞）有限公司
**************************************************************************/
int Turn(float gyro)
{
    static float Turn_Target,turn,Turn_Amplitude=54;
    float Kp=Turn_Kp,Kd=0;			//修改转向速度，请修改Turn_Amplitude即可
    //===================遥控左右旋转部分=================//
    if(1==Flag_Left)Turn_Target=-Turn_Amplitude/Flag_velocity;
    else if(1==Flag_Right)Turn_Target=Turn_Amplitude/Flag_velocity;
    else Turn_Target=0;
    if(1==Flag_front||1==Flag_back)Kd=Turn_Kd;
    else Kd=0;   //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
    //===================转向PD控制器=================//
    turn=Turn_Target*Kp/100+gyro*Kd/100+Move_Z;//结合Z轴陀螺仪进行PD控制
    //Move_Z=0;
    return turn;								 				 //转向环PWM右转为正，左转为负
}

/**************************************************************************
函数功能：CCD模式转向控制  巡线
入口参数：CCD提取的中线 Z轴陀螺仪
返回  值：转向控制PWM
作    者：平衡小车之家
**************************************************************************/
int CCD_turn(u8 CCD,float gyro)//转向控制
{
    float Turn;
    float Bias,kp=20,Kd=0.12f;       //30 0.12
    Bias=CCD-112;
    Turn=Bias*kp+gyro*Kd;
    if(!balance.Flag_Stop==0)Turn = 0;
    return Turn;
}
